import numpy as np


def wrap(theta):
    theta -= 2 * np.pi * np.floor((theta + np.pi) / (2 * np.pi))
    return theta


class FollowController:
    def __init__(self, kv, ki, kh):
        self._kv = kv
        self._ki = ki
        self._kh = kh

        self._e_sum = 0.0

    def ctrl(self, pose_des, pose_actual, d_des):
        e = np.linalg.norm(pose_des[:2] - pose_actual[:2]) - d_des

        theta_des = np.arctan2(pose_des[1] - pose_actual[1], pose_des[0] - pose_actual[0])

        v_des = self._kv * e + self._ki * self._e_sum

        omega_des = self._kh * wrap(theta_des - pose_actual[2])

        self._e_sum += e

        return v_des, omega_des
